
/*
 *
 * Copyright (c) 2006 P.J.Leonard
 * 
 * http://www.frinika.com
 * 
 * This file is part of Frinika.
 * 
 * Frinika is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.

 * Frinika is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.

 * You should have received a copy of the GNU General Public License
 * along with Frinika; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
package sane;

import java.util.logging.Level;
import java.util.logging.Logger;
import uk.ac.bath.domains.IP.IPModelIF;

/**
 *
 * @author pjl
 */
public class CartPole implements IPModelIF {

    float GRAVITY = 9.8f;
    float MASSCART = 1.0f;
    float MASSPOLE = 0.1f;
    float TOTAL_MASS = (MASSPOLE + MASSCART);
    float LENGTH = 0.5f;		  /* actually half the pole's length */

    float POLEMASS_LENGTH = (MASSPOLE * LENGTH);
    float FORCE_MAG = 10.0f;
    float TAU = 0.02f;		  /* seconds between state updates */

    private float force;
//    final float TWO_PI=(float) (Math.PI * 2.0);

    public CartPole(float x, float x_dot, float theta, float theta_dot) {
        this.x = x;
        this.x_dot = x_dot;
        this.theta = theta;
        this.theta_dot = theta_dot;
    }
    float FOURTHIRDS = 1.3333333333333f;
    float x;
    float x_dot;
    float theta;
    float theta_dot;

    public void reset(float x, float x_dot, float theta, float theta_dot) {
        this.x = x;
        this.x_dot = x_dot;
        this.theta = theta;
        this.theta_dot = theta_dot;
    }
    /*----------------------------------------------------------------------
    cart_pole:  Takes an action (0 or 1) and the current values of the
    four state variables and updates their values by estimating the state
    TAU seconds later.
    ----------------------------------------------------------------------*/

    void tick(int action) {
        float xacc, thetaacc, costheta, sintheta, temp;

        force = action*FORCE_MAG;
        costheta = (float) Math.cos(theta);
        sintheta = (float) Math.sin(theta);

        temp = (force + POLEMASS_LENGTH * theta_dot * theta_dot * sintheta) / TOTAL_MASS;

        thetaacc = (GRAVITY * sintheta - costheta * temp) / (LENGTH * (FOURTHIRDS - MASSPOLE * costheta * costheta / TOTAL_MASS));

        xacc = temp - POLEMASS_LENGTH * thetaacc * costheta / TOTAL_MASS;

        /*** Update the four state variables, using Euler's method. ***/
        x += TAU * x_dot;
        x_dot += TAU * xacc;
        theta += TAU * theta_dot;
        theta_dot += TAU * thetaacc;
    }

    public String toString() {
        return "x:" + x + " x_dot:" + x_dot + " phi:" + theta + " phi_dot:" + theta_dot;
    }

    public double getForce() {
        return force;
    }

    public double getPhi() {
        return Math.PI-theta;
    }

    public double getX() {
        return x;
    }

    public void setForce(double d) {
//        try {
//            throw new Exception();
//        } catch (Exception ex) {
//            Logger.getLogger(CartPole.class.getName()).log(Level.SEVERE, null, ex);
//        }
    }

    public void overRide(boolean b) {
        throw new UnsupportedOperationException("Not supported yet.");
    }
}
